import java.util.ArrayList;
import java.util.LinkedList;
import java.util.Stack;
import java.util.HashSet;
import java.util.PriorityQueue;
import java.util.Comparator;
import java.util.*;

public class AStar extends SearchAlgorithm
{
    private PriorityQueue<Node> openList;
    private PriorityQueue<Node> closedList;
   
    AStar()
    {
        openList = new PriorityQueue<Node>(100, new Comparator<Node> ()
                                                { public int compare(Node n1, Node n2)
                                                    { return n1.getState().getValue().compareTo(n2.getState().getValue());}});
        closedList = new PriorityQueue<Node>(100, new Comparator<Node> ()
                                                { public int compare(Node n1, Node n2)
                                                    { return n1.getState().getValue().compareTo(n2.getState().getValue());}});
    }// constructor

    public Stack<Node> search(Problem problem)
    {
        State initial = problem.getStartState();
        Node initialNode = problem.createNode(initial,0,0,0,null);
        openList.add(initialNode);
        numGenerated = 1;
       
        while (!openList.isEmpty())
        {
            Node current = openList.poll();
            closedList.add(current);   
           
            //Many implementations check this on generation to improve  speed a bit
            //but for consistency, we do goal checking on expansion
            if (problem.isGoalState(current.getState()))
                return getSolutionPath(current);
               
            ArrayList<Node> successors = current.getSuccessors(); 
           
            if (successors != null)
            {
                successors = expand(successors, current, problem);
            }
           
            for(Node node:successors)
            {
                openList.add(node);
            }
        }// expansion cycle
       
        return null; // no solution found
    }// search method 

    public ArrayList<Node> expand(ArrayList<Node> successors, Node current,Problem problem)
    {
        ArrayList<Node> temp = new ArrayList<Node>();
        for(Node node:successors)
        {
             node.setG(current.getDepth()+1);
             node.setH(problem.getHeuristicValue(node));
             node.setDepth(current.getDepth()+1);
             node.setParent(current);
             
             Node op = find(openList,node);
             Node closed = find(closedList,node);
             double ovalue =0;
             double cvalue =0;
             if(op != null)
             	ovalue = op.getG()+op.getH();
             	
             if(closed != null)
                cvalue = closed.getG()+current.getH();
             
             double nvalue = node.getG()+node.getH();
             
             //System.out.println(closedList.contains(node)+" "+openList.contains(node));
             
             if((op == null) && (closed == null))
             {
                temp.add(node);
             }else if(op != null)
             {
                 if(nvalue<ovalue && ovalue != 0)
                 {
                     openList.remove(node);
                     temp.add(node);
                 }
             }else if (closed != null)
             {
                 if(nvalue<cvalue)
                 {
                     closedList.remove(node);
                     temp.add(node);
                 }
             }
             numGenerated++;
        }
       
        return temp;
    }
    
    public Node find(PriorityQueue<Node> findin, Node thisnode)
    {
    	Iterator<Node> it = findin.iterator();
    
        while(it.hasNext())
        {
            Node check = it.next();
        	if(thisnode.getState().toString().equals(check.getState().toString()))
        	{
        		return check;
        	}
        }
        
        return null;
    }
}